% 不考虑逆时间模式
% 特别说明: GRBL的mc_line直线处理函数，没有做任何操作，也就是说直线对GRBL来说都采用直接过渡，直接做速度约束。
function grbl_simulate()
    %%%%%% 圆弧 %%%%%%
%     target  = [20,10,10];
%     postion = [10,10,10];
%     offset  = [5, 0, 0 ];
%     raduis  = sqrt( offset(1)^2 + offset(2)^2 );
%     turns   = -1;
%     mc_arc(target, postion, offset, raduis, turns);
    %%%%%% 速度前瞻-拐点速度求解 %%%%%%
    sys_position    = [0,0,0; 6,8,0; 20,0,0; 25,30,0; 30,2,0];
    steps_per_mm    = [100  ,  80,  60]; %1mm = 100个脉冲
    acc             = [12000,6000,3000]; %各个轴的加速度
    vec             = [10000,5000,3000]; %各个轴的速度
    n_axis          = length( steps_per_mm );
    [millimeters, rapid_rate, acceleration,junction_speed] = plan_buffer_line( sys_position, n_axis, steps_per_mm, acc, vec);
    %%%%%% 速度前瞻-前瞻与后顾 %%%%%%
    planner_recalculate(length(sys_position) - 1, rapid_rate, junction_speed, millimeters, acceleration);
    
end

% 圆弧插补仿真
% G2 X... Y... Z... I... J... K... [F...]
% 圆弧指令需要先用直线指令移动到起点，然后才能开始画圆
% 顺时针（CW）或逆时针（CCW）
% target  : 表示目标位置，XYZ三个轴。在GRBL中这个参数有Gcode指定
% postion ： 当前的坐标位置,XYZ
% offset  : 从当前位置到圆心的偏移量,IGK
% raduis  : 半径,以i=n,j=m,k=0为例，raduis = sqrt( (x-i)^2 + (y-j)^2 )
% plane   : 选择平面 x,y平面 z为线性轴，或者xz, yz平面，这里1xy;2xz;3yz;
% turns   ：大于0逆时针，小于0顺时针，绝对值-1表示圈数
% 总结    : 这个插补算法不要求终点一定在圆弧上，它的思路是计算
%           1 (起始点-圆心)(终点-圆心)两条连线之间的夹角θ。
%           2 通过弓高误差约束插入的弦，并计算弦长
%           3 总弧长 / 弦长 = 插补段数
%           4 θ / 插补段数 = 插补角度
%           5 使用圆的参数方程{x = r*cos(θ), y = r*sin(θ)} 计算得到插补线段末端的坐标点
% 参考文献 : https://blog.csdn.net/cln512/article/details/120748647
function mc_arc(target, postion, offset, raduis, turns)
    ARC_ANGULAR_TRAVEL_EPSILON = 0.0000005; % 最小浮点误差约束
    N_ARC_CORRECTION = 12; % 控制间隔多次才使用sin cos函数运算

    arc_tolerance = 0.002;% 插补公差mm
    % 理论上 2pi = 6.283185,但是由于浮点误差，可能会导致2pi = 6.283184或者2pi = 6.283186
    % 这样导致一个完整的圆弧被错认为是一个极小的圆弧，导致出错。

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%start%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 下面这部分的运算的主要目的是把圆心当成坐标原点，然后得到这个圆心坐标系下起始点和终点的坐标，方便求得(起始点-圆心)(终点-圆心)两条连线之间的夹角
    x_rv = -offset(1);
    y_rv = -offset(2);
    % 圆心坐标:起始点 - 偏移量
    x_center = postion(1) - x_rv;
    y_center = postion(2) - y_rv;
    % 目标点到圆心之间的距离
    x_rt = target(1) - x_center;
    y_rt = target(2) - y_center;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%start 求夹角%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % arctan(A) + arctan(B) = arctan(A+B/1-AB)
    % arctan(A) - arctan(B) = arctan(A-B/1+AB)
    % A = arctan(y/x)
    % B-A = arctan( ( y1/x1 - y0/x0 ) / ( 1 + ( y1/x1 * y0/x0 ) ) ) 
    %     = arctan( (x0y1 - x1y0) / (x0x1 + y0y1) )
    % 求(起始点-圆心)(终点-圆心)两条连线之间的夹角
    ybuf = x_rv*y_rt - x_rt*y_rv;
    xbuf = x_rv*x_rt + y_rt*y_rv;
    angular_travel = atan2( ybuf, xbuf );
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%start%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if( turns > 0 ) 
        if(angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON)% 逆时针旋转时弧度接近0认为是旋转了1周
            angular_travel = angular_travel + (2 * pi);
        end
    elseif( angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON ) % 顺时针旋转时弧度接近0认为是旋转了1周
        angular_travel = angular_travel - (2 * pi);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   
    % 参考 https://blog.csdn.net/cln512/article/details/120748647 的时候需要注意，angular_travel标识的是角度不是圆心到弓高之间的距离
    % 描述:
    % 画一个半径为r的圆
    % 画弦k,使得k的弓高为插补误差arc_tolerance
    % 过圆心做弦的垂线h(一定平分弦)
    % h = r - arc_tolerance
    % k^2 = r^2 - h^2 
    %     = r^2 - (r - arc_tolerance)^2
    %     = r^2 - (r^2 - 2*r*arc_tolerance + arc_tolerance^2) 
    %     = 2*r*arc_tolerance - arc_tolerance^2
    %     = arc_tolerance * ( 2 * r - arc_tolerance )
    if( 2 * raduis > arc_tolerance )                                % 圆弧直径必须大于插补公差
        arc_length = angular_travel * raduis / 2;                   % 这里计算的是向量夹角对应的弧的半径，这里除2是后面运算的预处理
        chord_length = arc_tolerance * (2 * raduis - arc_tolerance);% 计算插补线段的长度
        segments = abs( floor( arc_length / chord_length ) );
    else
        segments = 0;
    end
    if( segments > 0 )
        % 插补弦所在的弧对应角度
        theta_per_segment = angular_travel / segments;
        % 三角函数的泰勒展开
        % sin(x) = x - x^3/3! + (高阶无穷小(x^3))
        % cos(x) = 1 - x^2/2! + (高阶无穷小(x^2))
        cos_T = 2 - theta_per_segment * theta_per_segment;
        sin_T = theta_per_segment * 0.16666667 * ( cos_T + 4 );% 这里的近似计算是GRBL中直接提取出来的，应该是一个通过实验计算出来的式子
        cos_T = cos_T / 2; % 前面扩大了2倍，现在除回来
        count = 0;% 计数器，用于统计使用了泰勒展开式做近似计算的次数

        % 和差化积推导
        % e^ix = cosx + i*sinx
        % e^(a+b) = (cos(a) + i*sin(a))*(cos(b) + i*sin(b)) 
        %         = cos(a)cos(b) - sin(a)sin(b) + i*sin(a)cos(b) + i*sin(b)cos(a)
        % cos(a+b) = cos(a)cos(b) - sin(a)sin(b)
        % sin(a+b) = sin(a)cos(b) + sin(b)cos(a)

        % 在极坐标中,圆的参数方程为{x = r*cos(θ), y = r*sin(θ)}
        % φ = θ, 第一个扇区的角度是φ, 插入一个θ
        % x = r * cos(φ + θ) = r * [cos(φ)cos(θ) - sin(φ)sin(θ)]
        %       带入圆参数方程  x = r*cos(φ), y = r*sin(φ)
        %  = x * cos(θ) - y * sin(θ)

        % y = r * sin(φ + θ) = r * [sin(φ)cos(θ) + sin(θ)cos(φ)]
        %       带入圆参数方程 x = r*cos(φ), y = r*sin(φ)
        %   = y*cos(θ) + x*sin(θ)

        positions = zeros(segments + 1, 2);
        pos_x = x_center + x_rv;
        pos_y = y_center + y_rv;
        for loop = 0 : 1 : segments
            positions(loop+1, :) = [pos_x, pos_y]; % 为了画图准备的
            if( count < N_ARC_CORRECTION )
                r_axisi = y_rv * cos_T + x_rv * sin_T;
                x_rv = x_rv*cos_T - y_rv*sin_T;
                y_rv = r_axisi;
                count = count + 1;
            else
                % 这个运算以起始点为参考，计算插补loop次以后的坐标，而且用标准sin cos
                % 目的是为了矫正使用泰勒展开结果计算的累积误差
                cos_Ti = cos(loop * theta_per_segment);
                sin_Ti = sin(loop * theta_per_segment);
                x_rv = ( -offset(1) ) * cos_Ti - ( -offset(2) ) * sin_Ti;
                y_rv = ( -offset(2) ) * cos_Ti + ( -offset(1) ) * sin_Ti;
                count = 0;
            end
            pos_x = x_center + x_rv;
            pos_y = y_center + y_rv;
        end
        % figure;
        plot(positions(:, 1), positions(:, 2), 'b-o'); 
        hold on;  
        % 这个命令用于保持当前图像，使得接下来的绘图命令可以在同一个图上继续绘制，而不是创建一个新的图像。
        scatter(x_center, y_center, 'r*');  
        % 使用scatter函数在图中标记圆弧的圆心。x_center和y_center是圆心的x和y坐标。
        % 'r*' 是格式字符串，指定使用红色（red）和星号标记（star marker）来显示圆心位置。
        xlabel('X position');  
        % xlabel函数添加x轴的标签，这里的标签是 'X position'。
        ylabel('Y position');  
        % ylabel函数添加y轴的标签，这里的标签是 'Y position'。
        title('Arc path visualization');  
        % title函数给图像添加标题，这里的标题是 'Arc path visualization'。
        axis equal;  
        % axis equal命令调整坐标轴的刻度，使x轴和y轴的单位刻度长度相同。这样做可以确保图形的比例正确，圆形看起来不会变形。
        grid on;  
        % grid on命令开启网格，这有助于更准确地看到每个点的位置。
        hold off;  
        % hold off命令关闭保持状态，这意味着如果之后再进行绘图，MATLAB将在新的图像窗口中创建图像，而不是在当前图像上继续绘制。
    end
end

% 速度前瞻 - 拐角速度求解
% motion_path     : 运动路径,[[0,0,0],[1,1,1],[2,2,2]],从000出发过111到222
% n_axis          : 轴的数量
% steps_per_mm    : 1mm需要多少脉冲
% acceleration    : 各个轴的加速度
% axis_max_rate   : 各个轴的最大速度
% 参考文献         : https://blog.csdn.net/cln512/article/details/120748978?spm=1001.2014.3001.5502
% 总结            :GRBL的速度前瞻比较有意思，它的前瞻段数是缓存区中有多少个块，就前瞻多少个块。
%                 :1 先将mm坐标系换算成电机最小步进距离组成的坐标系
%                  2 将各个轴的位移计算出来,并且又换算成mm
%                  3 然后计算当前位移方向上各个轴的单位矢量的分量。
%                  4 然后计算出当前位移方向上各个轴的加速度，速度的单位矢量投射到各个轴上的分量 
%                  5 计算拐角速度，通过给定一个误差h确定假想圆的大小，这个假想圆与两个向量相切，于是圆的向心速度就是拐角速度。
function [bmillimeters, brapid_rate, bacceleration, bjunction_speed] = plan_buffer_line(motion_path, n_axis, steps_per_mm, acceleration, axis_max_rate)
    junction_deviation              = 0.01;%mm, 人为给定的用于控制圆弧大小的误差系数
    node_num                        = length(motion_path);
    previous_unit_vec               = zeros( n_axis );
    block_max_junction_speed_sqr    = zeros( node_num );        % 最大拐角速度
    block_millimeters               = zeros( node_num );        % block_millimeters  位移的模长
    block_acceleration              = zeros( node_num );
    block_rapid_rate                = zeros( node_num );
    sys_position                    = zeros( node_num );% 系统的初始位置

    % 仿真需要的预处理,把motion_path的第一个点转化成起始点 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for i = 1: 1 : n_axis
        sys_position(i) = round( motion_path(1, i) * steps_per_mm(i) );
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     figure;
    plot(motion_path(:, 1), motion_path(:, 2), 'b-o'); 
    hold on;  
    xlabel('X');  
    ylabel('Y');  
    zlabel('Z');  
    title('motion path');  
    grid on;  
    hold off;  
    for node = 2 : 1 : node_num
        target_steps             = zeros( n_axis,1 );
        block_steps              = zeros( n_axis,1 );
        unit_vec                 = zeros( n_axis,1 );
        block_dir                = zeros( n_axis,1 ); % 记录运动方向, 1反方向, 0正方向
        junction_unit_vec_buf    = zeros( n_axis,1 );
        block_step_event_count   = 0;
        for axis = 1: 1 : n_axis
            % 第一步
            % mm 坐标系转换成 脉冲步数坐标系
            target_steps(axis) = round( motion_path(node, axis) * steps_per_mm(axis) );      
            % 计算当前位置运动到目标位置的变换量
            delta_steps = target_steps(axis) - sys_position(axis);
            % 更新位置
            sys_position(axis) = target_steps(axis);

            if( delta_steps ~= 0 )
                block_steps(axis)      = abs( delta_steps );                            % 每个轴的位移
                if( block_steps(axis) > block_step_event_count )                        % 全部轴中最大的位移
                    block_step_event_count = block_steps;                               % TODO : 这里只是记录最大位移，仿真的时候没什么作用。
                end
                % 第二步
                unit_vec(axis)         = delta_steps / steps_per_mm(axis);              % 单个轴的单位向量
            else % 位置没有变化的轴
                block_steps(axis)      = 0;
                unit_vec(axis)         = 0;
            end
    
            % 记录运动方向
            block_dir(axis) = 0;
            if( unit_vec(axis) < 0 )
                block_dir(axis) = 1;
            end
        end

        % 单位向量描述了目标运动方向中各轴的参与比例。单位向量的每个分量代表了该方向中相应轴的相对贡献程度。
        % 例如，一个向量 [0.6,0.8,0] 表明在当前的运动方向中，Y轴的贡献大于X轴，而Z轴不参与。
        % block_millimeters = sqrt(x0^2 + y0^2 + z0^2)
        % new_unit_vec(0) = x0/sqrt(x0^2 + y0^2 + z0^2)
        % new_unit_vec(1) = y0/sqrt(x0^2 + y0^2 + z0^2)
        % new_unit_vec(2) = z0/sqrt(x0^2 + y0^2 + z0^2)
        % 第三步
        [block_millimeters(node-1),new_unit_vec] = convert_delta_vector_to_unit_vector( unit_vec );
%         fprintf("block_millimeters = %.10f\r\n",block_millimeters(node));
        % fprintf("new_unit_vec = %.10f,%.10f,%.10f\r\n",new_unit_vec(1),new_unit_vec(2),new_unit_vec(3));
        
        % 速度或者加速度 / 单位向量
        % 这个操作的核心是确定在不超过单个轴的最大速度或加速度限制的情况下，系统在该方向上能够达到的最大速度或加速度。
        % 物理意义： 给定一个方向，如果某个轴的单位向量分量较小，意味着该轴在整体运动中承担较小的部分。
        %           然而，如果这个轴的最大速度或加速度很低，即使其分量小，它也可能成为整体运动的限制因素。
        %           通过将轴的最大速度或加速度除以其单位向量分量，可以得出该轴在维持整体运动方向不变的情况下能提供的最大速度或加速度。
        %           这个结果告诉我们，为了不超过这个轴的限制，整个系统在这个方向上的最大速度或加速度应该是多少。
        % 这里能引出一个提速的方法:如果最慢的轴的速度的平方的2倍开根号大于速度最块的轴，那么可以考虑走45度角的斜线。
        % 第四步
        block_acceleration(node-1) = limit_acceleration_by_axis_maximum(new_unit_vec, acceleration);
        block_rapid_rate(node-1)   = limit_max_rate_by_axis_maximum( new_unit_vec, axis_max_rate );
        % 第五步
        % 拐角速度求解
        % 作图 :纵坐标为速度，横坐标为时间, 在第一象限上任意画一点A(x,y0)，连接原点O与A，然后画点B(2*x, 0),连接AB点
        % 这就是一段行程内速度的变化曲线，然后画第二段，画点C(2*x+n, y1),点D(2(2*x+n),0),连接BC，CD。
        % AB,BC看做是首尾相连的向量，B点就是速度的衔接点,向量拐角记作theta。
        % 向量夹角是共尾部后的夹角。所以速度拐角(junction_theta) = 180 - theta。
        % 向量夹角公式:cos<a,b> = ab/(|a|*|b|) 
        %                      = x1x2 + y1y2 + z1z2 / sqrt(x1*x1 + y1*y1 + z1*z1)*sqrt(x2*x2 + y2*y2 + z2*z2)
        % junction_cos_theta   = cos(180 - theta) 
        %                      = -cos(theta) 
        %                      = -(x1x2 + y1y2 + z1z2 / sqrt(x1*x1 + y1*y1 + z1*z1)*sqrt(x2*x2 + y2*y2 + z2*z2))
        % 由于是单位矢量,所以:sqrt(x1*x1 + y1*y1 + z1*z1) = sqrt(x2*x2 + y2*y2 + z2*z2)) = 1
        % junction_cos_theta = -(x1x2 + y1y2 + z1z2) 
        %                    = -x1x2 - y1y2 - z1z2
    
        junction_cos_theta = 0; % cos(θ)
        for i = 1 : 1 : n_axis
            junction_cos_theta = junction_cos_theta - ( new_unit_vec(i) * previous_unit_vec(i) ); 
            junction_unit_vec_buf(i) = new_unit_vec(i) - previous_unit_vec(i); %  向量减法;代表的是从上一个方向到当前方向需要发生的变化。
            previous_unit_vec(i) = new_unit_vec(i);
        end

        % 这里的思想是用圆弧的向速度作为拐角的过渡速度
        % 需要用到圆周运动的向心加速度的公式 : a_c = v^2 /r ;其中a是加速度;c是下标，意思是centripetal;v 是物体的线速度或切线速度;r 是圆的半径
        % 直线与圆相切，圆心和切点的连线垂直切线。
        % 作图:一个首尾相连的向量 Ventry、Vexit,它们连接处的夹角为theta;圆O与Ventry、Vexit相切;h是theta的角平分线上的一段距离，起点在向量的交点处，终点在内切圆上。
        % 大小人为给定，它控制着圆的大小。向量 Vjunction = Vexit - Ventry。
        % 二倍角公式:cos(θ) = 2 * cos^2(θ/2) - 1 = 1 - 2*sin^2(θ/2)
        % 化简能得到 sin(θ/2) = sqrt( (1 - cos(θ)) / 2 )
        % 结合图知道 sin(θ/2) = r / (r+h)
        % v^2 = a * r = acc * ( ( h * sin(θ/2) ) / ( 1 - sin(θ/2) ) )

        if( junction_cos_theta > 0.999999 )         %夹角为零度,拐角的衔接速度需要降到0
            block_max_junction_speed_sqr(node-1) = 0;
        elseif(junction_cos_theta < -0.999999)      %夹角为180,拐角的衔接速度不需要任何处理,把过渡速度设置成一个巨大的值，表示不参与后面的处理
            block_max_junction_speed_sqr(node-1) = 10^38;
        else
            [~,junction_unit_vec] = convert_delta_vector_to_unit_vector(junction_unit_vec_buf);
            junction_acceleration = limit_acceleration_by_axis_maximum(junction_unit_vec, acceleration);
            sin_theta_d2 = sqrt( 0.5 * (1 - junction_cos_theta) );% sin(θ/2)
            block_max_junction_speed_sqr(node-1) = ( junction_acceleration * junction_deviation * sin_theta_d2 / (1 - sin_theta_d2) );
        end
        fprintf("block_max_junction_speed_sqr = %.10f\r\n",block_max_junction_speed_sqr(node-1));
    end
    bmillimeters   = block_millimeters;
    brapid_rate    = block_rapid_rate;
    bacceleration  = block_acceleration;
    bjunction_speed = block_max_junction_speed_sqr;
end
function [magnitude,unit_vector] = convert_delta_vector_to_unit_vector(vector)
    % 经过这个计算 unit_vector中的各分量的合成矢量大小为1(单位矢量)
    asix_num = length(vector);
    unit_vector = zeros(asix_num);
    magnitude = 0;
    for i = 1 : 1 : asix_num
        magnitude = magnitude + vector(i)^2;
    end
    if( magnitude == 0 ) %如果合成矢量为0,不再做后面的计算
        return;
    end
    magnitude = sqrt( magnitude );  % 模长
    inv_magnitude = 1 / magnitude;  % 计算模长变成单位向量的缩放倍数
    for i = 1 : 1 : asix_num
        unit_vector(i) = vector(i) * inv_magnitude; % 缩小每个分量使得合成矢量是单位矢量
    end
end
function limit_value = limit_acceleration_by_axis_maximum(vector, acc)
    asix_num    = length(vector);
    limit_value = 10^38;
    for i = 1 : 1 : asix_num
        if( vector(i) ~= 0 )
            buf = acc(i) / vector(i);
            if( buf < limit_value )
                limit_value = buf;
            end
        end
    end
end
function limit_value = limit_max_rate_by_axis_maximum(vector, rate)
    asix_num    = length(vector);
    limit_value = 10^38;
    for i = 1 : 1 : asix_num
        if( vector(i) ~= 0 )
            buf = rate(i) / vector(i);
            if( buf < limit_value )
                limit_value = buf;
            end
        end
    end
end

% 速度前瞻 - 速度约束
% 前瞻算法不仅约束拐点速度，也对一段行程内的最大速度做约束。
% GRBL的速度约束模型是直接直接过渡的约束模型
% 参考文献 : 《数控系统连续小线段的路径优化与速度前瞻控制算法研究_聂伟刚》
%           《基于运动控制卡的PC数控进给速度前瞻控制_刘青山》
%           《嵌入式数控系统速度前瞻规划算法研究_游达章》
%            https://blog.csdn.net/cln512/article/details/120749226?spm=1001.2014.3001.5502
% 后顾速度约束的思想:
% 把末端速度当成是起始速度,向起始段的方向计算每一段，看该段上的距离和加速度能否达到所需要的末端速度
% 利用运动动学基本公式: V^2 = V0^2 + 2at。约束条件:V = min{设定速度,机器允许的最大速度,sqrt(V0^2 + 2at)}
% 这是直接过渡法的速度约束公式，前瞻也是用了同样的方法。
%%%%这个函数模型将期望速度设置为最大速度
% segments          : 路径的段数
% rapid_rate        : 该位移方向上的目标速度
% junction_speed    : 拐角速度             
% millimeters       : 位移长度             
% acc               : 加速度   

function planner_recalculate(segments, rapid_rate, junction_speed, millimeters, acc)
    % 预处理 计算 2at
    tow_at_buf = zeros(segments, 1);
    for seg = 1 : 1 : segments
        tow_at_buf(seg) = 2 * millimeters(seg) * acc(seg);
    end 
    % 后顾过程
    v_path_restriction = zeros(segments, 1);
    entry_speed_sqr    = zeros(segments, 1) + 1;
    % 计算每个段的入口速度的平方
    for seg = segments : -1 : 1
        % V^2 = V0^2 + 2at
        v_path_restriction(seg) = rapid_rate(seg)^2 + tow_at_buf(seg);
        if v_path_restriction(seg) > junction_speed(seg)^2
            entry_speed_sqr(seg+1) = junction_speed(seg)^2;
        else
            entry_speed_sqr(seg+1) = v_path_restriction(seg);
        end
    end

    % 前瞻过程
    for seg = 1 : 1 : segments
        % V^2 = V0^2 + 2at
        v_path_restriction(seg) = rapid_rate(seg)^2 + tow_at_buf(seg);
        if v_path_restriction(seg) > junction_speed(seg)^2
            if entry_speed_sqr(seg+1) > junction_speed(seg)^2
                entry_speed_sqr(seg+1) = junction_speed(seg)^2;
            end
        else
            if entry_speed_sqr(seg+1) > v_path_restriction(seg)
                entry_speed_sqr(seg+1) = v_path_restriction(seg);
            end
        end
    end
    % entry_speed_sqr - 是速度的平方
    % 绘制图形并标记每一个点
    figure;
    plot(1 : segments + 1, entry_speed_sqr, 'LineWidth', 2, 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k');
    title('Entry Speed Squared per Segment');
    xlabel('Segment Number');
    ylabel('Entry Speed Squared (V^2)');
    grid on;
end












